#include "robotjointgroup.h"

RobotJointGroup::RobotJointGroup(NameGroup names, QString description,
                                 RobotComponent *parent, float radius,
                                 QVector3D offset, bool invertLength) :
    RobotComponent(JOINT_GROUP, names, radius, description, parent, offset, invertLength)
{}

RobotJointGroup::~RobotJointGroup(){
    //Remove all of the joints and deallocate the memory
    QMap<int, RobotJoint*>::iterator i;
    RobotJoint* removedJoint;

    for(i = _joints.begin(); i != _joints.end(); i++){
        //Deallocate the memory used by the joint
        removedJoint = i.value();
        free(removedJoint);
    }
}

void RobotJointGroup::addJoint(int id, float min, float max, QVector3D rotationAxis,
                               QString description){
    RobotJoint* newJoint = (RobotJoint*)malloc(sizeof(RobotJoint));

    newJoint->id = id;
    newJoint->min = min;
    newJoint->max = max;
    newJoint->curAngle = 0;
    newJoint->rotationAxis = rotationAxis;
    newJoint->description = new QString(description);

    _joints.insert(id, newJoint);
}

void RobotJointGroup::removeJoint(int jointID){
    //Remove the joint from our map and deallocate memory
    RobotJoint* removedJoint = _joints.take(jointID);

    if(removedJoint != NULL)
        free(removedJoint);
}

bool RobotJointGroup::containsJoint(int jointID) const{
    return _joints.contains(jointID);
}

const QList<int> RobotJointGroup::jointIDs() const{
    QList<int> retList;
    QMap<int, RobotJoint*>::const_iterator i;

    for(i = _joints.begin(); i != _joints.end(); i++){
        retList.append(i.key());
    }

    return retList;
}


float RobotJointGroup::setAngle(RobotJoint* joint, float theta){
    //Correct for minimum and maximum angles
    if(theta > joint->max)
        joint->curAngle = joint->max;
    else if(theta < joint->min)
        joint->curAngle = joint->min;
    else
        joint->curAngle = theta;

    return joint->curAngle;
}


float RobotJointGroup::setJointAngle(int jointID, float theta){
    RobotJoint* joint;

    if(_joints.contains(jointID)){
        joint = _joints.value(jointID);
        return setAngle(joint, theta);
    }

    return INVALID_JOINT;
}

float RobotJointGroup::moveJointBy(int jointID, float delta){
    float newTheta;
    RobotJoint* joint;

    if(_joints.contains(jointID)){
        joint = _joints.value(jointID);

        newTheta = joint->curAngle + delta;
        return setAngle(joint, newTheta);
    }
    else
        return INVALID_JOINT;
}

float RobotJointGroup::jointAngle(int jointID) const{
    RobotJoint* joint;

    if(_joints.contains(jointID)){
        joint = _joints.value(jointID);
        return joint->curAngle;
    }
    else
        return INVALID_JOINT;
}

//TODO: Refactor so that there is more code reuse with
//functions that access joint information?
QString RobotJointGroup::jointDescription(int jointID) const{
    RobotJoint* joint;

    if(_joints.contains(jointID)){
        joint = _joints.value(jointID);
        return *(joint->description);
    }

    return QString();
}

float RobotJointGroup::minimum(int jointID) const{
    RobotJoint* joint;

    if(containsJoint(jointID)){
        joint = _joints.value(jointID);
        return joint->min;
    }

    return INVALID_JOINT;
}

float RobotJointGroup::maximum(int jointID) const{
    RobotJoint* joint;

    if(containsJoint(jointID)){
        joint = _joints.value(jointID);
        return joint->max;
    }

    return INVALID_JOINT;
}

QQuaternion RobotJointGroup::rotation() const{
    //Multiply the quaternions together for each joint
    QQuaternion retValue;

    QMap<int, RobotJoint*>::const_iterator i;
    RobotJoint* curJoint;

    for(i = _joints.begin(); i != _joints.end(); i++){
        curJoint = i.value();

        retValue *= QQuaternion::fromAxisAndAngle(curJoint->rotationAxis, curJoint->curAngle);
    }

    return retValue.normalized();
}
